#include "IPathPlanner.h"

#include <irrlicht.h>
#include <list>
#include <vector>
#include "GraphNode.h"
#include "SparseGraph.h"
#include "UtilityFunctions.h"

using namespace irr;
using namespace core;


IPathPlanner::IPathPlanner():m_bStart(false),m_bFinish(false),
		m_dCellWidth(0),m_dCellHeight(0),m_iCellsX(25),m_iCellsY(25),
		m_iSourceCell(0),m_iTargetCell(0),m_dCostToTarget(0.0),m_pGraph(NULL) {}

bool IPathPlanner::PlotPathToTarget(vector2df start, vector2df target, std::list<vector3df>& path){
	//Find target and source node Id's
	int targetNodeId = Graph_FindNodeIdContainingVector(*m_pGraph, target, floor, m_iCellsX, m_iCellsY);
	if(targetNodeId != -1)
		printf("Target position found in node %i\n", targetNodeId);

	int startNodeId = Graph_FindNodeIdContainingVector(*m_pGraph, start, floor, m_iCellsX, m_iCellsY);
	if(startNodeId != -1)
		printf("Start position found in node %i\n", startNodeId);
	
	std::list<int> m_Path;
	typedef Graph_SearchAStar<NavGraph, Heuristic_Euclid> AStarSearch;

	//Todo: Figure out link error in GetPathTotarget()
	AStarSearch AStar(*m_pGraph, startNodeId, targetNodeId);
	m_Path = AStar.GetPathToTarget();

	std::list<int>::iterator nodeIdIter;
	for(nodeIdIter = m_Path.begin(); nodeIdIter != m_Path.end(); ++nodeIdIter)
	{
		NavGraphNode<> node = m_pGraph->getNode(*nodeIdIter);
		core::vector3df moveposition(node.getPosition().X, 0, node.getPosition().Y);
		path.push_back(moveposition);
	}
	return true;
}

void IPathPlanner::LoadLevel(scene::ISceneNode* level){
	room = level->getTransformedBoundingBox();

	vector3df* ptrEdges;
	vector3df edges[8];

	ptrEdges = &edges[0];
	room.getEdges(edges);

	for(int i = 0; i < 8; ++i)
	{
		if(edges[i].Y < 0)
		{
			floor.addInternalPoint(edges[i].X, edges[i].Z);
		}
	}

	if(floor.isValid())
	{
		dimension2df roomSize = floor.getSize();
		dimension2df cellSize(roomSize.Width/m_iCellsX, roomSize.Height/m_iCellsY);
		m_dCellWidth = cellSize.Width;
		m_dCellHeight = cellSize.Height;
		CreateGraph(m_iCellsX, m_iCellsY);
	}
}

void IPathPlanner::CreateGraph(int numberCellsX, int numberCellsY)
{
	if(m_pGraph)
		delete m_pGraph;

	m_pGraph = new NavGraph(false);

	CreateGrid(*m_pGraph, floor, numberCellsX, numberCellsY);

}

void IPathPlanner::AddObstacle(scene::ISceneNode* node)
{
	
	aabbox3df objBB = node->getTransformedBoundingBox();
	vector3df* ptrEdges;
	vector3df edges[8];
	ptrEdges = &edges[0];
	objBB.getEdges(edges);
	rectf blockedSpace;
	dimension2df space;
	stringw node_name = node->getName();
	
	for(int i = 0; i < 8; ++i)
	{
		printf("Edge: %i -> < %f, %f, %f >\n", i, edges[i].X, edges[i].Y, edges[i].Z);
		if(edges[i].Y < 10)
		{
			blockedSpace.addInternalPoint(edges[i].X, edges[i].Z);
		
		}
	}

	printf("Adding obstacale %ls with area of %f by %f\n", node_name.c_str(), blockedSpace.getWidth(), blockedSpace.getHeight());
	Graph_AddObstacle(*m_pGraph, objBB);
}